﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text;
using Microsoft.Xna.Framework;
using System.Collections;

namespace SpaceGame.Gameplay.AI.Pathfinding
{
    class PathPlanner
    {
        /// <summary>
        /// Keep reference to world to access navigation mesh and other dynamic actors.
        /// </summary>
        private World world;

        /// <summary>
        /// entity (ship) we are path planning for.
        /// </summary>
        private Entity entity;

        /// <summary>
        /// Algorithm for pathfinding.
        /// </summary>
        private AStar pathFinder;

        /// <summary>
        /// Holds current planned macro path.
        /// </summary>
        private List<MeshNode> plannedPath;

        private Random random = new Random();
        
        
        #region Properties
        /// <summary>
        /// Returns true if a goal has been defined.
        /// </summary>
        public bool IsGoalDefined
        {
            get; private set;
        }

        /// <summary>
        /// Position to achieve.
        /// </summary>
        public Vector2 Goal
        {
            get; private set;
        }

        /// <summary>
        /// A subgoal is a target that is the same convex polygon of the entity, and hence best path is just a straight line.
        /// </summary>
        public Vector2 SubGoal
        {
            get; private set;
        }


        #endregion

        #region Initialize
        public PathPlanner(Entity entity, World world)
        {
            if(entity == null) throw new ArgumentNullException("entity");
            if(world == null) throw new ArgumentNullException("world");

            this.world = world;
            this.entity = entity;

            //instanciates A* implemenation with defined heuristics
            IPathHeuristic heuristic = new EuclideanPathHeuristic(new ObstaclesDensityPathHeuristic(world), (a, b) => a*b/2);
            pathFinder = new AStar(heuristic);
        }
        #endregion

        /// <summary>
        /// Determine target position.
        /// </summary>
        /// <param name="goal"></param>
        public void SetGoal(Vector2 goal)
        {
            Goal = goal;
            IsGoalDefined = true;

            PlanPathToGoal();
            
        }

        /// <summary>
        /// Cancels previous set target position.
        /// </summary>
        public void CancelGoal()
        {
            IsGoalDefined = false;
            Goal = Vector2.Zero;
        }

        /// <summary>
        /// Path planner will currently use A* to search for best path.
        /// </summary>
        private void PlanPathToGoal()
        {
            MeshNode goalNode = world.NavigationMesh.FindEnclosingMeshNode(Goal);
            MeshNode currentNode = world.NavigationMesh.FindEnclosingMeshNode(entity.Position);
            
            if (!goalNode.Equals(currentNode))
            {
                plannedPath = pathFinder.FindPath(entity.Position, Goal, currentNode, goalNode);
                
                //if can't find feasible path
                if(plannedPath == null)
                {
                    CancelGoal(); //TODO: add timer or tabu list to keep preventing from trying
                }
            }
            else
            {   
                //reseting path, because we can just use straight line   
                plannedPath = null;
                SubGoal = Goal;
            }
        }


        /// <summary>
        /// Asks path planner wich direction to go to according to current set goal.
        /// </summary>
        /// <returns></returns>
        public Vector2 GetSubGoal()
        {
            //if there is no planned path
            if (plannedPath == null || plannedPath.Count == 0)
            {
                return SubGoal;
            }  

            //check if goal is visible
            if (world.IsInLineOfSight(entity.Position, Goal))
            {
                SubGoal = Goal;
                //plannedPath = null;
                return Goal;
            }

            //check where we are in the planned path
            MeshNode currentMesh = world.NavigationMesh.FindEnclosingMeshNode(entity.Position);


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			int indexOfMeshInPath = 0; //plannedPath.FindIndex(m => m.Equals(currentMesh));

			for (int i = 0; i < plannedPath.Count; i++)
			{
				if (plannedPath[i].Equals(currentMesh))
				{
					indexOfMeshInPath = i;
					break;
				}
			}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


			//something went terribly wrong and we're currently in unplanned mesh: rebuild path and try again
			if (indexOfMeshInPath == -1)
			{
				PlanPathToGoal();
				return SubGoal;
			}

            //we're at the last mesh node: plan was completed
            if(indexOfMeshInPath == plannedPath.Count -1)
            {
                SubGoal = Goal;
                plannedPath = null;
                return SubGoal;
            }

            //goes in direction of next mesh
            MeshNode nextMesh = plannedPath[indexOfMeshInPath + 1];

			int index = 0;

			for (int i = 0; i < nextMesh.Neighbors.Count; i++)
			{
				if (nextMesh.Neighbors[i].Equals(currentMesh))
				{
					index = i;
					break;
				}
			}

            //int index = nextMesh.Neighbors.FindIndex(m => m.Equals(currentMesh));
            
            //we can use centroid or midpoint in boundaries
            var newgoal = nextMesh.Boundaries[index].MidPoint ;
        //  var newgoal = nextMesh.Neighbors[index].Polygon.Centroid;

            //todo: avoid getting stuck at midpoints
            
            //trim path to reflect current position
            //if (indexOfMeshInPath > 0)
            //    plannedPath.RemoveRange(0, indexOfMeshInPath + 1);

            SubGoal = newgoal;
           
            return SubGoal;
        }
    }
}
